#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "PWM.h"
#include "Motor.h"
#include "Serial.h"
#include "string.h"
#include "MPU6050.h"
#include "Timer.h"
#include "inv_mpu.h"
#include "Encoder.h"
#include "Pid.h"



uint16_t fps=0,timercount=0,fpsshow=0;
uint8_t i;
float Pitch,Roll,Yaw,Temp1=0,Temp2=0;
int16_t Real_Speed1,Target_Speed1,Real_Position1=0,Target_Position1,moto1,
		Real_Speed2,Target_Speed2,Real_Position2=0,Target_Position2,moto2;
		Last_Yaw1=0,Last_Yaw2,roundcount1=0,roundcount2=0;


int16_t Get_Target_Position1(void)
{
	Temp1=Yaw;

		if(Last_Yaw1>140 && Temp1<-140)
		{
			roundcount1++;
		}
		if(Last_Yaw1<-140 && Temp1>140)
		{
			roundcount1--;
		}

		Last_Yaw1=Temp1;

		if(roundcount1%2!=0 && roundcount1>0)
		{
			Temp1=180+Temp1;
		}
		else if(roundcount1%2==0 && roundcount1>0)
		{
			Temp1=Temp1-180;
		}
		else if(roundcount1%2!=0 && roundcount1<0)
		{
			Temp1=Temp1-180;
		}
		else if(roundcount1%2==0 && roundcount1<0)
		{
			Temp1=180+Temp1;
		}
	return (Temp1/180)*540+roundcount1*540;
}


int main(void)
{
	float R1,R2;
	Serial_Init();
	Timer1_Init();
	OLED_Init();	
	MPU_Init();
	i=mpu_dmp_init();
	if(i)
	{
		OLED_ShowString(1, 10, "erro:");
		OLED_ShowNum(1, 15, i, 1);
	}
	Motor_Init(1000,36);
	Encoder1_Init();
	Encoder2_Init();

	OLED_ShowString(1, 1, "fps:");
	OLED_ShowString(2, 1, "A:");
	OLED_ShowString(3, 1, "B:");
	while(1)
	{
		R1=(Real_Position1/520.0)*180.0;
		R2=(Real_Position2/520.0)*180.0;
		OLED_ShowNum(1, 5, fpsshow, 3);
		OLED_ShowSignedNum(2, 3, R1, 3);
		OLED_ShowSignedNum(3, 3, R2, 3);

		while(mpu_dmp_get_data(&Pitch, &Roll, &Yaw));	//必须要用while等待，才能读取成功
        // MPU_Get_Accelerometer(&AX,&AY, &AZ);		//得到加速度传感器数据
        // MPU_Get_Gyroscope(&GX, &GY, &GZ);		//得到陀螺仪数据

		Target_Position1=Get_Target_Position1();
		Target_Position2=(Roll/180)*540;

		

		

        // printf("%d,%.1f,%.1f,%d,%d,%d,%d\n",roundcount1,Pitch,Yaw,fpsshow,Real_Position2,Target_Position2,moto1);
		
		fps++;
	}
}



void TIM1_UP_IRQHandler(void)//10ms进一次
{
	if (TIM_GetITStatus(TIM1, TIM_IT_Update) == SET)
	{
		timercount++;
		if(timercount==100)//fps计
		{
			timercount=0;
			fpsshow=fps;
			fps=0;
		}
		moto1=Control1();
		Control2();

		TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
	}
}
